/***************************************************************************************************************************
* drone_control_utils.h
*
* Author: Wxt
*
* Update Time: 2021.7.28
*
* 主要功能：1.打印上层控制指令
*                       2.打印无人机状态
*                       3.打印位置控制器输出结果
*                       4.【获取当前时间函数】 单位：秒
*                       5.【坐标系旋转函数】- 机体系到enu系
*                       6.计算位置误差，计算速度误差？
***************************************************************************************************************************/
#ifndef tello_CONTORL_UTILS_H
#define tello_CONTORL_UTILS_H

#include <Eigen/Eigen>
#include <math.h>
#include <math_utils.h>
#include <uav_drone_msgs/Message.h>
#include <uav_drone_msgs/ControlCommand.h>
#include <uav_drone_msgs/SwarmCommand.h>
#include <uav_drone_msgs/DroneState.h>
#include <uav_drone_msgs/PositionReference.h>
#include <uav_drone_msgs/AttitudeReference.h>
#include <uav_drone_msgs/ControlOutput.h>
#include <uav_drone_msgs/LogMessage.h>
#include <uav_drone_msgs/LogMessageControl.h>
#include <uav_drone_msgs/AttitudeTarget_replace.h>
#include <uav_drone_msgs/PositionTarget_replace.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>

using namespace std;

#define NUM_POINT 2



namespace tello_control_utils 
{


// 打印上层控制指令  
void printf_command_control(const uav_drone_msgs::ControlCommand& _ControlCommand)
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>> Control Command <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(NUM_POINT);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout << "Source: [ "<< _ControlCommand.source << " ]  Command_ID: "<< _ControlCommand.Command_ID <<endl;

    switch(_ControlCommand.Mode)
    {
        case uav_drone_msgs::ControlCommand::Idle:
                cout << "Command: [ Idle ] " <<endl;

            break;

        case uav_drone_msgs::ControlCommand::Takeoff:
            cout << "Command: [ Takeoff ] " <<endl;
            break;

        case uav_drone_msgs::ControlCommand::Hold:
            cout << "Command: [ Hold ] " <<endl;
            break;

        case uav_drone_msgs::ControlCommand::Land:
            cout << "Command: [ Land ] " <<endl;
            break;

        case uav_drone_msgs::ControlCommand::Move:
            if(_ControlCommand.Reference_State.Move_mode == uav_drone_msgs::PositionReference::XYZ_POS)
            {
                cout << "Command: [ Move ] " << "Move_mode: [ XYZ_POS ] " <<endl;
            }else if(_ControlCommand.Reference_State.Move_mode == uav_drone_msgs::PositionReference::XY_POS_Z_VEL)
            {
                cout << "Command: [ Move ] " << "Move_mode: [ XY_POS_Z_VEL ] " <<endl;
            }else if(_ControlCommand.Reference_State.Move_mode == uav_drone_msgs::PositionReference::XY_VEL_Z_POS)
            {
                cout << "Command: [ Move ] " << "Move_mode: [ XY_VEL_Z_POS ] " <<endl;
            }else if(_ControlCommand.Reference_State.Move_mode == uav_drone_msgs::PositionReference::XYZ_VEL)
            {
                cout << "Command: [ Move ] " << "Move_mode: [ XYZ_VEL ] " <<endl;
            }else if(_ControlCommand.Reference_State.Move_mode == uav_drone_msgs::PositionReference::TRAJECTORY)
            {
                cout << "Command: [ Move ] " << "Move_mode: [ TRAJECTORY ] " <<endl;
            }

            if(_ControlCommand.Reference_State.Move_frame == uav_drone_msgs::PositionReference::ENU_FRAME)
            {
                cout << "Move_frame: [ ENU_FRAME ] " <<endl;
            }else if(_ControlCommand.Reference_State.Move_frame == uav_drone_msgs::PositionReference::BODY_FRAME)
            {
                cout << "Move_frame: [ BODY_FRAME ] " <<endl;
            }

            cout << "Position [X Y Z] : " << _ControlCommand.Reference_State.position_ref[0] << " [ m ] "<< _ControlCommand.Reference_State.position_ref[1]<<" [ m ] "<< _ControlCommand.Reference_State.position_ref[2]<<" [ m ] "<<endl;
            cout << "Velocity [X Y Z] : " << _ControlCommand.Reference_State.velocity_ref[0] << " [m/s] "<< _ControlCommand.Reference_State.velocity_ref[1]<<" [m/s] "<< _ControlCommand.Reference_State.velocity_ref[2]<<" [m/s] "<<endl;
            cout << "Acceleration [X Y Z] : " << _ControlCommand.Reference_State.acceleration_ref[0] << " [m/s^2] "<< _ControlCommand.Reference_State.acceleration_ref[1]<<" [m/s^2] "<< _ControlCommand.Reference_State.acceleration_ref[2]<<" [m/s^2] "<<endl;

            cout << "Yaw : "  << _ControlCommand.Reference_State.yaw_ref* 180/M_PI << " [deg] " <<endl;

            break;

        case uav_drone_msgs::ControlCommand::Disarm:
            cout << "Command: [ Disarm ] " <<endl;
            break;

        case uav_drone_msgs::ControlCommand::User_Mode1:
            cout << "Command: [ User_Mode1 ] " <<endl;
            break;
        
        case uav_drone_msgs::ControlCommand::User_Mode2:
            cout << "Command: [ User_Mode2 ] " <<endl;
            break;
    }

}



// 打印无人机状态
void prinft_drone_state(const uav_drone_msgs::DroneState& _Drone_state)
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>>>   Drone State   <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(NUM_POINT);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout << "Time: " << _Drone_state.time_from_start <<" [s] ";

    //是否和飞控建立起连接
    if (_Drone_state.connected == true)
    {
        cout << " [ Connected ]";
    }
    else
    {
        cout << " [ Unconnected ]";
    }

    //是否上锁
    if (_Drone_state.armed == true)
    {
        cout << " [ Armed ]";
    }
    else
    {
        cout << " [ DisArmed ]";
    }

    //是否在地面
    if (_Drone_state.landed == true)
    {
        cout << " [ Ground ] ";
    }
    else
    {
        cout << " [ Air ] ";
    }

    cout << "[ " << _Drone_state.mode<<" ] " <<endl;

    cout << "Position [X Y Z] : " << _Drone_state.position[0] << " [ m ] "<< _Drone_state.position[1]<<" [ m ] "<<_Drone_state.position[2]<<" [ m ] "<<endl;
    cout << "Velocity [X Y Z] : " << _Drone_state.velocity[0] << " [m/s] "<< _Drone_state.velocity[1]<<" [m/s] "<<_Drone_state.velocity[2]<<" [m/s] "<<endl;
    cout << "Attitude [R P Y] : " << _Drone_state.attitude[0] * 180/M_PI <<" [deg] "<<_Drone_state.attitude[1] * 180/M_PI << " [deg] "<< _Drone_state.attitude[2] * 180/M_PI<<" [deg] "<<endl;
    cout << "Att_rate [R P Y] : " << _Drone_state.attitude_rate[0] * 180/M_PI <<" [deg/s] "<<_Drone_state.attitude_rate[1] * 180/M_PI << " [deg/s] "<< _Drone_state.attitude_rate[2] * 180/M_PI<<" [deg/s] "<<endl;
}

// 打印位置控制器输出结果
void prinft_attitude_reference(const uav_drone_msgs::AttitudeReference& _AttitudeReference)
{
    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(NUM_POINT);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout << "Attitude_sp [R P Y]  : " << _AttitudeReference.desired_attitude[0] * 180/M_PI <<" [deg]  "<<_AttitudeReference.desired_attitude[1] * 180/M_PI << " [deg]  "<< _AttitudeReference.desired_attitude[2] * 180/M_PI<<" [deg] "<<endl;
    cout << "Throttle_sp [ 0-1 ]  : " << _AttitudeReference.desired_throttle <<endl;
}

void prinft_ref_pose(const geometry_msgs::PoseStamped& ref_pose)
{
    cout <<">>>>>>>>>>>>>>>>>>>>>>> Ref Pose <<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;

    //固定的浮点显示
    cout.setf(ios::fixed);
    //setprecision(n) 设显示小数精度为n位
    cout<<setprecision(NUM_POINT);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);
      
    cout << "Ref_position [X Y Z] : " << ref_pose.pose.position.x <<" [m] "<< ref_pose.pose.position.y <<" [m] " << ref_pose.pose.position.z <<" [m] "<<endl;
}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 其 他 函 数 <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 

// 【获取当前时间函数】 单位：秒
float get_time_in_sec(const ros::Time& begin_time)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec - begin_time.sec;
    float currTimenSec = time_now.nsec / 1e9 - begin_time.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

// 【坐标系旋转函数】- 机体系到enu系
// body_frame是机体系,enu_frame是惯性系，yaw_angle是当前偏航角[rad]
void rotation_yaw(float yaw_angle, float body_frame[2], float enu_frame[2])
{
    enu_frame[0] = body_frame[0] * cos(yaw_angle) - body_frame[1] * sin(yaw_angle);
    enu_frame[1] = body_frame[0] * sin(yaw_angle) + body_frame[1] * cos(yaw_angle);
}

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 控 制 辅 助 函 数 <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 
//计算位置误差
Eigen::Vector3f cal_pos_error(const uav_drone_msgs::DroneState& _DroneState, const uav_drone_msgs::PositionReference& _Reference_State)
{
    Eigen::Vector3f pos_error;

    for (int i=0; i<3; i++)
    {
        pos_error[i] = _Reference_State.position_ref[i] - _DroneState.position[i];
    }

    return pos_error;
}

//计算速度误差
Eigen::Vector3f cal_vel_error(const uav_drone_msgs::DroneState& _DroneState, const uav_drone_msgs::PositionReference& _Reference_State)
{
    Eigen::Vector3f vel_error;

    for (int i=0; i<3; i++)
    {
        vel_error[i] = _Reference_State.velocity_ref[i] - _DroneState.velocity[i];
    }

    return vel_error;
}

Eigen::Vector3d accelToThrust(const Eigen::Vector3d& accel_sp, float mass, float tilt_max)
{
      //tello暂时不需要计算油门
}

Eigen::Vector3d thrustToThrottle(const Eigen::Vector3d& thrust_sp)
{
        //tello暂时不需要计算油门
}

//Throttle to Attitude
//Thrust to Attitude
//Input: desired thrust (desired throttle [0,1]) and yaw_sp(rad)
//Output: desired attitude (quaternion)
uav_drone_msgs::AttitudeReference ThrottleToAttitude(const Eigen::Vector3d& thr_sp, float yaw_sp)
{
        //tello不需要根据油门计算至太
}

//random number Generation
//if a = 0 b =0, random_num = [-1,1]
//rand函数，C语言中用来产生一个随机数的函数
float random_num(float a, float b)
{
    float random_num;
    
    random_num = a * 2 * (((float)(rand() % 100))/100 - 0.5) + b;

    return random_num;
}

}
#endif
